﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace PlanetenGravity
{
    public class Collision<T>
    {
        public Collision(T t1, T t2)
        {
            Test(t1 as Sprite, t2 as Sprite);
        }
        public bool Test(Sprite a, Sprite b)
        {
            Vector2 distance = b.Position - a.Position;
            float radii = b.Radius + a.Radius;

            if (distance.Length() > radii)
                return false;

            a.Collided();
            b.Collided();

            float ima = (a.Mass > 0 && a.Mass < 100) ? 1 / a.Mass : 0;
            float imb = (b.Mass > 0 && b.Mass < 100) ? 1 / b.Mass : 0;

            float penetration = radii - distance.Length();
            distance.Normalize();

            Vector2 seperation = distance * (penetration / (ima + imb));

            a.Position -= ima * seperation;
            b.Position -= imb * seperation;

            float impact = Vector2.Dot(b.Speed - a.Speed, distance);
            if (impact > 0)
                return true;

            Vector2 impulse = distance * -(1 + 0.95f) * impact / (ima + imb);

            // Change momentum of each object
            a.Speed -= ima * impulse;
            b.Speed += imb * impulse;

            return true;
        }
    }
}
